// 创建一个生命周期动作服务器，这个服务器将接收客户端的请求并根据请求进行状态转换

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "test_interfaces/action/progress.hpp"

using namespace std::placeholders;                                    // 占位符
using test_interfaces::action::Progress;                              // 简化Progress类型的引用
using GoalHandleProgress = rclcpp_action::ServerGoalHandle<Progress>; // 类型别名

// 在ROS2的生命周期管理中，LifecycleNodeInterface::CallbackReturn 是一个枚举类，
// 用于表示生命周期节点回调函数的返回状态。该状态指示节点是否成功执行某个生命周期转换操作，
// 比如从"未配置"到"已配置"，或从"激活"到"非激活"等
// CallbackReturn 枚举值：
// 1. SUCCESS : 回调成功完成，可以继续生命周期转换
// 2. FAILURE : 回调失败，生命周期转换将停止，节点不会进入目标状态
// 3. ERROR   : 出现不可恢复的错误，生命周期管理器可以选择关闭或重启节点
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

class LifecycleActionServer : public rclcpp_lifecycle::LifecycleNode
{
public:
    LifecycleActionServer()
        : rclcpp_lifecycle::LifecycleNode("lifecycle_action_server")
    {
    }

    // lifecycle 配置节点的操作
    CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;

    // lifecycle 激活节点操作
    CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;

    // lifecycle 停用节点操作
    CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;

private:
    rclcpp_action::Server<test_interfaces::action::Progress>::SharedPtr action_server_;

    // action 的 处理目标请求
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID &uuid,
        std::shared_ptr<const Progress::Goal> goal);

    // action 的 处理取消请求
    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandleProgress> goal_handle);

    // action 的 处理已接受的请求并执行转换任务
    void handle_accepted(const std::shared_ptr<GoalHandleProgress> goal_handle)
    {
        std::thread{std::bind(&LifecycleActionServer::execute, this, _1), goal_handle}.detach();
    }

    void execute(const std::shared_ptr<GoalHandleProgress> goal_handle);
};

// 配置节点的操作
CallbackReturn LifecycleActionServer::on_configure(const rclcpp_lifecycle::State &)
{

    RCLCPP_INFO(get_logger(), "Configuring node ...");

    // 配置成功，返回success
    return CallbackReturn::SUCCESS;
}

// 激活节点操作
CallbackReturn LifecycleActionServer::on_activate(const rclcpp_lifecycle::State &)
{

    RCLCPP_INFO(get_logger(), "Activating node...");

    // 3-1 创建动作服务端
    this->action_server_ = rclcpp_action::create_server<Progress>(
        this,
        "get_sum",
        std::bind(&LifecycleActionServer::handle_goal, this, _1, _2), // 动作请求
        std::bind(&LifecycleActionServer::handle_cancel, this, _1),   // 处理任务取消请求
        std::bind(&LifecycleActionServer::handle_accepted, this, _1)  // 接受任务请求并开始执行

    );

    RCLCPP_INFO(get_logger(), "动作端服务器创建成功");

    return CallbackReturn::SUCCESS;
}

// 停用节点操作
CallbackReturn LifecycleActionServer::on_deactivate(const rclcpp_lifecycle::State &)
{

    RCLCPP_INFO(get_logger(), "Deactivating node ...");

    return CallbackReturn::SUCCESS;
}

// action 的 处理目标请求
rclcpp_action::GoalResponse LifecycleActionServer::handle_goal(
    const rclcpp_action::GoalUUID &uuid,
    std::shared_ptr<const Progress::Goal> goal)
{
    (void)uuid;
    RCLCPP_INFO(this->get_logger(), "接收到动作客户端请求，请求数字为 %ld", goal->num);
    if (goal->num < 1)
    {
        return rclcpp_action::GoalResponse::REJECT;
    }
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

// action 的 处理取消请求
rclcpp_action::CancelResponse LifecycleActionServer::handle_cancel(
    const std::shared_ptr<GoalHandleProgress> goal_handle)
{
    (void)goal_handle;
    RCLCPP_INFO(this->get_logger(), "Received cancel request");
    return rclcpp_action::CancelResponse::ACCEPT;
}

void LifecycleActionServer::execute(const std::shared_ptr<GoalHandleProgress> goal_handle)
{
    RCLCPP_INFO(this->get_logger(), "开始执行任务");
    rclcpp::Rate loop_rate(10.0);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Progress::Feedback>();
    auto result = std::make_shared<Progress::Result>();
    int64_t sum = 0;
    for (int i = 1; (i <= goal->num) && rclcpp::ok(); i++)
    {
        sum += i;
        // 检查是否有一个取消请求
        if (goal_handle->is_canceling())
        {
            result->sum = sum;
            goal_handle->canceled(result);
            RCLCPP_INFO(this->get_logger(), "任务取消");
            return;
        }

        feedback->progress = (double_t)i / goal->num;
        goal_handle->publish_feedback(feedback);
        RCLCPP_INFO(this->get_logger(), "连续反馈中，进度： %.2f", feedback->progress);

        loop_rate.sleep();
    }

    if (rclcpp::ok())
    {
        result->sum = sum;
        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(), "任务完成！");
    }
}

/////////////////////////////////////
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto action_server_node = std::make_shared<LifecycleActionServer>();

    // 使用生命周期管理
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(action_server_node->get_node_base_interface());
    executor.spin();

    rclcpp::shutdown();
    return 0;
}